// #pragma once
#ifndef ESVO2_CORE_FACTOR_POSELOCAL_H
#define ESVO2_CORE_FACTOR_POSELOCAL_H

#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include <esvo2_core/factor/utility.h>
namespace esvo2_core
{
namespace factor
{
class PoseLocalParameterization : public ceres::LocalParameterization
{
public:
    ~PoseLocalParameterization() {};
    bool Plus(const double *x, const double *delta, double *x_plus_delta) const
    {
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
    } ;
    bool ComputeJacobian(const double *x, double *jacobian) const
    {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
        j.topRows<6>().setIdentity();
        j.bottomRows<1>().setZero();

    return true;
    };
    int GlobalSize() const { return 7; };
    int LocalSize() const  { return 6; };
};
}
}

#endif
